/**
 * @copyright (C) COPYRIGHT 2022 Fortiortech Shenzhen
 * @file      MotorControl.c
 * @author    Fortiortech  Appliction Team
 * @since     Create:2021-04-10
 * @date      Last modify:2022-07-14
 * @note      Last modify author is Marcel He
 * @brief
 */

#include <MyProject.h>
/* Private variables ----------------------------------------------------------------------------*/

MotStateType data mcState;
MotStaM McStaSet;

/**
 * @brief     电机控制状态机
 * @warning   电机的状态只能在电机状态控制中切换，禁止在其他地方切换电机状态
 * @date      2022-07-14
 */
void MC_Control(void)
{
    switch (mcState)
    {
    case mcReady:
        Motor_Ready();

        if (mcFaultSource != FaultNoSource)
        {
            mcState = mcFault;
        }
        else if ((mcCurOffset.OffsetFlag == 1) && (isCtrlPowOn == true))
        {
            mcState = mcInit;
            mcCurOffset.OffsetFlag = 0;
            mcCurOffset.OffsetCount = 0;
        }
        else
        {
        }

        break;

    case mcInit:
        if (mcFaultSource != FaultNoSource)
        {
            mcState = mcFault;
        }
        else if (isCtrlPowOn == false)
        {
            mcState = mcStop;
        }
        else if (mcCurOffset.OffsetFlag == 1)
        {
            PI_Init();
            PI2_Init();
            PI3_Init();
            DRV_DR = MOTOR_STAT0;
            DRV_CMR = 0x001;       /* 下桥输出占空比 */
            ClrBit(DRV_CR, OCS);   /* 比较值来源选择 */
            SetBit(DRV_CR, DRVEN); /* 计数器使能 */
            MOE = 1;
            mcState = mcRun;
        }

        break;

    case mcRun:
        if (mcFaultSource != FaultNoSource)
        {
            mcState = mcFault;
        }
        else if (isCtrlPowOn == false)
        {
            mcState = mcStop;
        }
        else
        {
        }

        break;

    case mcStop:
        if (mcFaultSource != FaultNoSource)
        {
            mcState = mcFault;
        }
        else
        {
            MOE = 0;
            ClrBit(DRV_CR, FOCEN);
            mcState = mcReady;
        }

        break;

    case mcFault:
        if (mcFaultSource == FaultNoSource) // 保护清零后关闭输出，软件重新运行
        {
            mcState = mcReady;
        }
        else
        {
            ClrBit(DRV_CR, FOCEN); // FOC Disable
            MOE = 0;
        }

        break;

    default:
        mcState = mcReady;
        break;
    }
}
